/*
	math.cpp

	Author: Chris Haag
	Description:

		Contains misc. math functions

		Orthonormalize function converted from

		http://members.nbci.com/_XMCM/gdemont/e3d_html/engine_3d-math__adb.htm
*/

#include "stair.h"
#include "stairmath.h"
#include <math.h>
#include <memory.h>


void CalculateNormal(const _VERTEX* a, const _VERTEX* b, const _VERTEX* c, 
					 _NORMAL& n)
{
	_VERTEX v1, v2;

	// v1 = b - a
	v1.x = b->x - a->x;
	v1.y = b->y - a->y;
	v1.z = b->z - a->z;

	// v2 = c - a
	v2.x = c->x - a->x;
	v2.y = c->y - a->y;
	v2.z = c->z - a->z;

	// Take the cross product
	n.a = v1.y*v2.z - v1.z*v2.y;
	n.b = v1.z*v2.x - v1.x*v2.z;
	n.c = v1.x*v2.y - v1.y*v2.x;

	// Normalize the result
	Normalize(n);

	// Calculate the distance from the plane to the
	// origin, and negate it.
	n.d = -(n.a * a->x + n.b * a->y + n.c * a->z);
}


// Rotates a point about an arbitrary normalized vector
void Rotate_Arbitrary(_VERTEX* v, const _VECTOR* vAxis, const _VERTEX* vCenter, const float radians)
{
	float m[3][3]; // 3x3 matrix
	float x,y,z;
	float x2,y2,z2;
	float sinus = radians;
	float cosinus = radians;
	float V = 1 - cosinus;
	float a = vAxis->x;
	float b = vAxis->y;
	float c = vAxis->z;

	x = v->x - vCenter->x;
	y = v->y - vCenter->y;
	z = v->z - vCenter->z;

	SIN(sinus);
	COS(cosinus);

	m[0][0] = cosinus + (1-cosinus)*a*a;
	m[0][1] = (1-cosinus)*a*b + sinus*c;
	m[0][2] = (1-cosinus)*a*c - sinus*b;

	m[1][0] = (1-cosinus)*b*a - sinus*c;
	m[1][1] = cosinus + (1-cosinus)*b*b;
	m[1][2] = (1-cosinus)*b*c + sinus*a;

	m[2][0] = (1-cosinus)*c*a + sinus*b;
	m[2][1] = (1-cosinus)*c*b - sinus*a;
	m[2][2] = cosinus + (1-cosinus)*c*c;

	x2 = (x) * m[0][0] + (y) * m[1][0] + (z) * m[2][0];
	y2 = (x) * m[0][1] + (y) * m[1][1] + (z) * m[2][1];
	z2 = (x) * m[0][2] + (y) * m[1][2] + (z) * m[2][2];

	v->x = x2 + vCenter->x;
	v->y = y2 + vCenter->y;
	v->z = z2 + vCenter->z;
}

/* Create a matrix that will project the desired shadow. */
void shadowMatrix(GLfloat shadowMat[4][4],
  _NORMAL& vnGround,
  _VECTOR& vLight)
{
  GLfloat dot;

  /* Find dot product between light position vector and ground plane normal. */
  dot = vnGround.a * vLight.x +
    vnGround.b * vLight.y +
    vnGround.c * vLight.z;

  shadowMat[0][0] = dot - vLight.x * vnGround.a;
  shadowMat[1][0] = 0.f - vLight.x * vnGround.b;
  shadowMat[2][0] = 0.f - vLight.x * vnGround.c;
  shadowMat[3][0] = 0.f - vLight.x * vnGround.d;

  shadowMat[0][1] = 0.f - vLight.y * vnGround.a;
  shadowMat[1][1] = dot - vLight.y * vnGround.b;
  shadowMat[2][1] = 0.f - vLight.y * vnGround.c;
  shadowMat[3][1] = 0.f - vLight.y * vnGround.d;

  shadowMat[0][2] = 0.f - vLight.z * vnGround.a;
  shadowMat[1][2] = 0.f - vLight.z * vnGround.b;
  shadowMat[2][2] = dot - vLight.z * vnGround.c;
  shadowMat[3][2] = 0.f - vLight.z * vnGround.d;

  shadowMat[0][3] = 0.f;
  shadowMat[1][3] = 0.f;
  shadowMat[2][3] = 0.f;
  shadowMat[3][3] = dot;
}

CMatrix3x3::CMatrix3x3(void)
{
	ZeroMatrix();
}

CMatrix3x3::CMatrix3x3(float i11, float i12, float i13,
					   float i21, float i22, float i23,
					   float i31, float i32, float i33)
{
	m_f[0] = i11; m_f[3] = i12; m_f[6] = i13;
	m_f[1] = i21; m_f[4] = i22; m_f[7] = i23;
	m_f[2] = i31; m_f[5] = i32; m_f[8] = i33;
}

void CMatrix3x3::Invert()
{
	CMatrix3x3 mInv;
	int iCurRow, iCurCol;

	mInv.Identity();

	for (iCurCol = 0; iCurCol < 3; iCurCol++)
	{
		float fValue;
		int i;

		// Step one: Find which row we want the 1
		// to be in for this column and multiply that
		// row by the inverse of the value that should
		// be 1.
		fValue = GetElement(iCurCol, iCurCol);

		if (fValue != 1) { // Optimization here
			for (i=0; i < 3; i++)
			{
				SetElement(iCurCol, i, GetElement(iCurCol,i) / fValue);
				mInv(iCurCol,i) /= fValue;
			}
		}

		// Step two: Make all other rows of this column
		// to be zero
		for (iCurRow = 0; iCurRow < 3; iCurRow++)
		{
			if (iCurRow == iCurCol) continue;

			fValue = GetElement(iCurRow, iCurCol);
			
			// Do for all columns in this row
			for (i=0; i < 3; i++)
			{
				SetElement(iCurRow, i, GetElement(iCurRow,i) - GetElement(iCurCol,i) * fValue);
				mInv(iCurRow,i) = mInv(iCurRow,i) - mInv(iCurCol,i) * fValue;

			}
		}
	}
	memcpy(m_f, mInv.GetRawElements(), 3*3*sizeof(float));
}

void CMatrix3x3::ZeroMatrix()
{
	memset(m_f, 0, 3*3*sizeof(float));
}

void CMatrix3x3::Identity()
{
	ZeroMatrix();
	m_f[0] = m_f[4] = m_f[8] = 1;
}

void CMatrix3x3::Orthonormalize()
{
	float dot1, dot2, fLen;

    // dot1:= m(1,1) * m(2,1) + m(1,2) * m(2,2) + m(1,3) * m(2,3);
    // dot2:= m(1,1) * m(3,1) + m(1,2) * m(3,2) + m(1,3) * m(3,3);
	dot1 = GetElement(0,0) * GetElement(1,0) +
		GetElement(0,1) * GetElement(1,1) +
		GetElement(0,2) * GetElement(1,2);
	dot2 = GetElement(0,0) * GetElement(2,0) +
		GetElement(0,1) * GetElement(2,1) +
		GetElement(0,2) * GetElement(2,2);

    //m(1,1) := m(1,1) - dot1 * m(2,1) - dot2 * m(3,1);
    //m(1,2) := m(1,2) - dot1 * m(2,2) - dot2 * m(3,2);
    //m(1,3) := m(1,3) - dot1 * m(2,3) - dot2 * m(3,3);
	SetElement(0,0, GetElement(0,0) - dot1 * GetElement(1,0) - dot2 * GetElement(2,0));
	SetElement(0,1, GetElement(0,1) - dot1 * GetElement(1,1) - dot2 * GetElement(2,1));
	SetElement(0,2, GetElement(0,2) - dot1 * GetElement(1,2) - dot2 * GetElement(2,2));
    
	//vlen:= 1.0 / sqrt(m(1,1) * m(1,1) +
	//                       m(1,2) * m(1,2) +
	//                       m(1,3) * m(1,3));
	fLen = GetElement(0,0) * GetElement(0,0) +
		GetElement(0,1) * GetElement(0,1) +
		GetElement(0,2) * GetElement(0,2);
	SQRT(fLen);
	fLen = 1.0f / fLen;

	// m(1,1):= m(1,1) * vlen;
    // m(1,2):= m(1,2) * vlen;
    // m(1,3):= m(1,3) * vlen;
	SetElement(0,0, GetElement(0,0) * fLen);
	SetElement(0,1, GetElement(0,1) * fLen);
	SetElement(0,2, GetElement(0,2) * fLen);

	// dot1:= m(2,1) * m(1,1) + m(2,2) * m(1,2) + m(2,3) * m(1,3);
    // dot2:= m(2,1) * m(3,1) + m(2,2) * m(3,2) + m(2,3) * m(3,3);
	dot1 = GetElement(1,0) * GetElement(0,0) +
		GetElement(1,1) * GetElement(0,1) +
		GetElement(1,2) * GetElement(0,2);
	dot2 = GetElement(1,0) * GetElement(2,0) +
		GetElement(1,1) * GetElement(2,1) +
		GetElement(1,2) * GetElement(2,2);

	// m(2,1) := m(2,1) - dot1 * m(1,1) - dot2 * m(3,1);
    // m(2,2) := m(2,2) - dot1 * m(1,2) - dot2 * m(3,2);
    // m(2,3) := m(2,3) - dot1 * m(1,3) - dot2 * m(3,3);
	SetElement(1,0, GetElement(1,0) - dot1 * GetElement(0,0) - dot2 * GetElement(2,0));
	SetElement(1,1, GetElement(1,1) - dot1 * GetElement(0,1) - dot2 * GetElement(2,1));
	SetElement(1,2, GetElement(1,2) - dot1 * GetElement(0,2) - dot2 * GetElement(2,2));


    // vlen:= 1.0 / sqrt(m(2,1) * m(2,1) +
    //                   m(2,2) * m(2,2) +
    //                   m(2,3) * m(2,3));
	fLen = GetElement(1,0) * GetElement(1,0) +
		GetElement(1,1) * GetElement(1,1) +
		GetElement(1,2) * GetElement(1,2);
	SQRT(fLen);
	fLen = 1.0f / fLen;

	// m(2,1):= m(2,1) * vlen;
    // m(2,2):= m(2,2) * vlen;
    // m(2,3):= m(2,3) * vlen;
	SetElement(1,0, GetElement(1,0) * fLen);
	SetElement(1,1, GetElement(1,1) * fLen);
	SetElement(1,2, GetElement(1,2) * fLen);

	// m(3,1):= m(1,2) * m(2,3) - m(1,3) * m(2,2);
    // m(3,2):= m(1,3) * m(2,1) - m(1,1) * m(2,3);
    // m(3,3):= m(1,1) * m(2,2) - m(1,2) * m(2,1);
	SetElement(2,0, GetElement(0,1) * GetElement(1,2) - GetElement(0,2) * GetElement(1,1));
	SetElement(2,1, GetElement(0,2) * GetElement(1,0) - GetElement(0,0) * GetElement(1,2));
	SetElement(2,2, GetElement(0,0) * GetElement(1,1) - GetElement(0,1) * GetElement(1,0));
}

